package spark.math;

public class Matrix {
	public static final int DIMENSION = 4;
	public static final int FIELDS = DIMENSION * DIMENSION;
	public static final int _11 = 0;
	public static final int _12 = 1;
	public static final int _13 = 2;
	public static final int _14 = 3;
	public static final int _21 = 4;
	public static final int _22 = 5;
	public static final int _23 = 6;
	public static final int _24 = 7;
	public static final int _31 = 8;
	public static final int _32 = 9;
	public static final int _33 = 10;
	public static final int _34 = 11;
	public static final int _41 = 12;
	public static final int _42 = 13;
	public static final int _43 = 14;
	public static final int _44 = 15;

	private float matrix[];

	private Matrix() {
	}

	public Matrix(final Matrix source) {
		matrix = new float[FIELDS];
		System.arraycopy(source.matrix, 0, matrix, 0, FIELDS);
	}

	public static Matrix createIdentity() {
		Matrix identityMatrix = new Matrix();
		identityMatrix.matrix = new float[FIELDS];

		for(int i = 0; i < FIELDS; i++) {
			identityMatrix.matrix[i] = 0;
		}
		identityMatrix.matrix[_11] = identityMatrix.matrix[_22] = identityMatrix.matrix[_33] = identityMatrix.matrix[_44] = 1.0f;

		return identityMatrix;
	}

	public static Matrix createRotationX(float angle) {
		float matrix[] = new float[FIELDS];

		//  |  1  0       0       0 |
		//M=|  0  cos(A) -sin(A)  0 |
		//  |  0  sin(A)  cos(A)  0 |
		//  |  0  0       0       1 |
		matrix[_11] = 1;
		matrix[_21] = 0;
		matrix[_31] = 0;
		matrix[_41] = 0;
		matrix[_12] = 0;
		matrix[_22] = Geometry.cos(angle);
		matrix[_32] = -Geometry.sin(angle);
		matrix[_42] = 0;
		matrix[_13] = 0;
		matrix[_23] = Geometry.sin(angle);
		matrix[_33] = Geometry.cos(angle);
		matrix[_43] = 0;
		matrix[_14] = 0;
		matrix[_24] = 0;
		matrix[_34] = 0;
		matrix[_44] = 1;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createRotationY(float angle) {
		float matrix[] = new float[FIELDS];

		//  |  cos(A)  0  sin(A)  0 |
		//M=|  0       1  0       0 |
		//  | -sin(A)  0  cos(A)  0 |
		//  |  0       0  0       1 |
		matrix[_11] = Geometry.cos(angle);
		matrix[_21] = 0;
		matrix[_31] = Geometry.sin(angle);
		matrix[_41] = 0;
		matrix[_12] = 0;
		matrix[_22] = 1;
		matrix[_32] = 0;
		matrix[_42] = 0;
		matrix[_13] = -Geometry.sin(angle);
		matrix[_23] = 0;
		matrix[_33] = Geometry.cos(angle);
		matrix[_43] = 0;
		matrix[_14] = 0;
		matrix[_24] = 0;
		matrix[_34] = 0;
		matrix[_44] = 1;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createRotationZ(float angle) {
		float matrix[] = new float[FIELDS];

		//  |  cos(A)  -sin(A) 0  0 |
		//M=|  sin(A)   cos(A) 0  0 |
		//  |  0        0      1  0 |
		//  |  0        0      0  1 |
		matrix[_11] = Geometry.cos(angle);
		matrix[_21] = -Geometry.sin(angle);
		matrix[_31] = 0;
		matrix[_41] = 0;
		matrix[_12] = Geometry.sin(angle);
		matrix[_22] = Geometry.cos(angle);
		matrix[_32] = 0;
		matrix[_42] = 0;
		matrix[_13] = 0;
		matrix[_23] = 0;
		matrix[_33] = 1;
		matrix[_43] = 0;
		matrix[_14] = 0;
		matrix[_24] = 0;
		matrix[_34] = 0;
		matrix[_44] = 1;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createRotationYawPitchRoll(float yaw, float pitch, float roll) {
		float cosX = (float) Math.cos(pitch);
		float sinX = (float) Math.sin(pitch);
		float cosY = (float) Math.cos(yaw);
		float sinY = (float) Math.sin(yaw);
		float cosZ = (float) Math.cos(roll);
		float sinZ = (float) Math.sin(roll);

		float yawPitchRoll[] = {
				cosY * cosZ + sinX * sinY * sinZ, -cosX * sinZ, sinX * cosY * sinZ - sinY * cosZ, 0,
				cosY * sinZ - sinX * sinY * cosZ, cosX * cosZ, -sinY * sinZ - sinX * cosY * cosZ, 0,
				cosX * sinY, sinX, cosX * cosY, 0,
				0, 0, 0, 1};

		Matrix result = new Matrix();
		result.matrix = yawPitchRoll;
		return result;
	}

	public Matrix createRotationQuaternion(float x, float y, float z, float w) {
		float matrix[] = new float[FIELDS];

		double xx = x * x;
		double xy = x * y;
		double xz = x * z;
		double xw = x * w;
		double yy = y * y;
		double yz = y * z;
		double yw = y * w;
		double zz = z * z;
		double zw = z * w;

		matrix[_11] = (float) (1.0 - 2.0 * (yy + zz));
		matrix[_12] = (float) (2.0 * (xy + zw));
		matrix[_13] = (float) (2.0 * (xz - yw));
		matrix[_21] = (float) (2.0 * (xy - zw));
		matrix[_22] = (float) (1.0 - 2.0 * (xx + zz));
		matrix[_23] = (float) (2.0 * (yz + xw));
		matrix[_31] = (float) (2.0 * (xz + yw));
		matrix[_32] = (float) (2.0 * (yz - xw));
		matrix[_33] = (float) (1.0 - 2.0 * (xx + yy));
		matrix[_14] = matrix[_24] = matrix[_34] = matrix[_41] = matrix[_42] = matrix[_43] = 0.0f;
		matrix[_44] = 1.0f;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createTranslation(float x, float y, float z) {
		float matrix[] = new float[FIELDS];

		//  | 1  0  0  X |
		//M=| 0  1  0  Y |
		//  | 0  0  1  Z |
		//  | 0  0  0  1 |
		matrix[_11] = 1;
		matrix[_21] = 0;
		matrix[_31] = 0;
		matrix[_41] = x;
		matrix[_12] = 0;
		matrix[_22] = 1;
		matrix[_32] = 0;
		matrix[_42] = y;
		matrix[_13] = 0;
		matrix[_23] = 0;
		matrix[_33] = 1;
		matrix[_43] = z;
		matrix[_14] = 0;
		matrix[_24] = 0;
		matrix[_34] = 0;
		matrix[_44] = 1;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createTranslation(final Vector3 translation) {
		return createTranslation(translation.x, translation.y, translation.z);
	}

	public static Matrix createScaling(float x, float y, float z) {
		float matrix[] = new float[FIELDS];

		//  | X  0  0  0 |
		//M=| 0  Y  0  0 |
		//  | 0  0  Z  0 |
		//  | 0  0  0  1 |
		matrix[_11] = x;
		matrix[_21] = 0;
		matrix[_31] = 0;
		matrix[_41] = 0;
		matrix[_12] = 0;
		matrix[_22] = y;
		matrix[_32] = 0;
		matrix[_42] = 0;
		matrix[_13] = 0;
		matrix[_23] = 0;
		matrix[_33] = z;
		matrix[_43] = 0;
		matrix[_14] = 0;
		matrix[_24] = 0;
		matrix[_34] = 0;
		matrix[_44] = 1;

		Matrix result = new Matrix();
		result.matrix = matrix;
		return result;
	}

	public static Matrix createScaling(final Vector3 scaling) {
		return createScaling(scaling.x, scaling.y, scaling.z);
	}

	public float determinant() {
		return
				matrix[_41] * matrix[_32] * matrix[_23] * matrix[_14] -
				matrix[_31] * matrix[_42] * matrix[_23] * matrix[_14] -
				matrix[_41] * matrix[_22] * matrix[_33] * matrix[_14] +
				matrix[_21] * matrix[_42] * matrix[_33] * matrix[_14] +
				matrix[_31] * matrix[_22] * matrix[_43] * matrix[_14] -
				matrix[_21] * matrix[_32] * matrix[_43] * matrix[_14] -
				matrix[_41] * matrix[_32] * matrix[_13] * matrix[_24] +
				matrix[_31] * matrix[_42] * matrix[_13] * matrix[_24] +
				matrix[_41] * matrix[_12] * matrix[_33] * matrix[_24] -
				matrix[_11] * matrix[_42] * matrix[_33] * matrix[_24] -
				matrix[_31] * matrix[_12] * matrix[_43] * matrix[_24] +
				matrix[_11] * matrix[_32] * matrix[_43] * matrix[_24] +
				matrix[_41] * matrix[_22] * matrix[_13] * matrix[_34] -
				matrix[_21] * matrix[_42] * matrix[_13] * matrix[_34] -
				matrix[_41] * matrix[_12] * matrix[_23] * matrix[_34] +
				matrix[_11] * matrix[_42] * matrix[_23] * matrix[_34] +
				matrix[_21] * matrix[_12] * matrix[_43] * matrix[_34] -
				matrix[_11] * matrix[_22] * matrix[_43] * matrix[_34] -
				matrix[_31] * matrix[_22] * matrix[_13] * matrix[_44] +
				matrix[_21] * matrix[_32] * matrix[_13] * matrix[_44] +
				matrix[_31] * matrix[_12] * matrix[_23] * matrix[_44] -
				matrix[_11] * matrix[_32] * matrix[_23] * matrix[_44] -
				matrix[_21] * matrix[_12] * matrix[_33] * matrix[_44] +
				matrix[_11] * matrix[_22] * matrix[_33] * matrix[_44];
	}

	public static Matrix multiply(final Matrix left, final Matrix right) {
		float newMatrix[] = new float[FIELDS];
		float sum;
		int x, y, i;
		for(x = 0; x < DIMENSION; x++) {
			for(y = 0; y < DIMENSION; y++) {
				sum = 0;
				for(i = 0; i < 4; i++) {
					sum += left.matrix[y + 4 * i] * right.matrix[4 * x + i];
				}
				newMatrix[y + 4 * x] = sum;
			}
		}

		Matrix result = new Matrix();
		result.matrix = newMatrix;
		return result;
	}

	public void multiply(final Matrix aMatrix) {
		float newMatrix[] = new float[FIELDS];
		float sum;
		int x, y, i;
		for(x = 0; x < DIMENSION; x++) {
			for(y = 0; y < DIMENSION; y++) {
				sum = 0;
				for(i = 0; i < 4; i++) {
					sum += matrix[y + 4 * i] * aMatrix.matrix[4 * x + i];
				}
				newMatrix[y + 4 * x] = sum;
			}
		}
		matrix = newMatrix;
	}

	public float[] getMatrix() {
		return matrix;
	}
}
